/* --------------------------- (C) COPYRIGHT 2022 Fortiortech ShenZhen -----------------------------
    File Name      : PLLInitandStateSwitch.c
    Author         : Fortiortech  Appliction Team
    Version        : V1.0
    Date           : 2022-7-23
    Description    : This file contains PLLInitandStateSwitch.c function used for Motor Startup.
----------------------------------------------------------------------------------------------------
                                       All Rights Reserved
------------------------------------------------------------------------------------------------- */

#include <FU68xx_2.h>
#include <Myproject.h>

/*  -------------------------------------------------------------------------------------------------
    Function Name  : PLLFunctionInit
    Description    : PLL参数初始化
    Date           : 2022-07-23
    Parameter      : None
    ------------------------------------------------------------------------------------------------- */
void PLLFunctionInit(void)
{
    SetBit(DRV_SR, DCIP);                                     // 0--1个载波周期执行一次foc中断，1--2个载波周期执行一次foc中断
    memset(&PLLfunction, 0, sizeof(PLLVar));
    
    MCCtrl.InitDelayFlag = 0;
    ClrBit(TIM2_CR1, T2EN);
    ClrBit(TIM3_CR1, T3EN); 
    
    PI_PLL_Init();
    FOC__THETA = 0;
    PLLfunction.Theta_pre = 0;
    
    FOC_EFREQACC  = 0;
    FOC_EFREQMIN  = 0;
    FOC_EFREQHOLD = 0;
    ClrBit(FOC_CR1, EFAE);
    ClrBit(FOC_CR1, RFAE);
    ClrBit(FOC_CR1, ANGM);

    PLLfunction.Angle_Compen = ANGLE_COM;
    LPF1_K = 3000;
    LPF2_K = 3000;
    LPF3_K = 500;
    PLLfunction.BEMFBase = V_factor1;
    PLLfunction.Vfactor = V_factor;
    MUL1_MB = PLLfunction.Vfactor;
    PLLfunction.PLLFunctionFlag = 1;
    PLLfunction.BEMFBase_Update_Count = 0;
    PLLfunction.BEMFBase_Update_Count1 = 0;
    FOC_IQREF = mcFocCtrl.mcIqref;
}

/*  -------------------------------------------------------------------------------------------------
    Function Name  : StartupState
    Description    : 启动状态机控制
    Date           : 2022-07-23
    Parameter      : None
    ------------------------------------------------------------------------------------------------- */

void PLLStateFunction(void)
{
    switch (PLLfunction.PLLFunctionFlag)
    {
        case 1:
            PLLfunction.Drv_Count++;

            if (PLLfunction.Drv_Count < 2)
            {
                                   
                if (PLLfunction.count < 30000)          //强拖函数
                {
                    PLLfunction.count++;
                }
                if(PLLfunction.count<6)
                {
                    PLLfunction.test = PLLfunction.Theta_pre + 10;                                       //Ç¿ÍÏº¯Êý
                } 
                else
                {
                    PLLfunction.test = PLLfunction.Theta_pre + 10;                                       //Ç¿ÍÏº¯Êý
                }                
                    PLLSoftwareFunction1();
                }
            else
            {
                PLLSoftwareFunction2(PLLfunction.Angle_Compen);                                                       //软件PLL函数

                if ((PLLfunction.count > 450) && (PLLfunction.Speed_PLL > CLOSE_SPEED))              //转速大于切环转速开始计数
                {
                    PLLfunction.Switch_Count++;
                   if  (PLLfunction.Switch_Count > 460)
                    {
                        GP05 = 0;
                        PLLfunction.count = 0;
                        PLLfunction.PLLFunctionFlag = 2;
                        PLLfunction.Theta_err = FOC__THETA - FOC__ETHETA;
                        ClrBit(DRV_SR , DCIP);
                        SetBit(FOC_CR1, ANGM);
                        
                    }  
                    else  if  (PLLfunction.Switch_Count > 480)
                    {
                        GP05 = 1;
                        mcFocCtrl.mcIqref = IQ_RUN_CURRENT;
                        PLLfunction.Angle_Compen = ANGLE_COM1;
                        FOC_IQREF = mcFocCtrl.mcIqref;
                    }                    
                }
                else
                {
                    if (PLLfunction.Switch_Count > 0)
                    {
                        PLLfunction.Switch_Count--;
                    }
                }
                PLLfunction.Drv_Count = 0;
            }
            break;
        case 2:
            if (PLLfunction.count < 1024)                                                       //角度平滑切换
            {
                PLLfunction.count++;
                FOC__THETA = FOC__ETHETA + ((PLLfunction.Theta_err >> 10)*(1024 - PLLfunction.count));              
            }
            else
            {
                PLLfunction.PLLFunctionFlag = 3;
            }
             if(PLLfunction.count>300) 
             {     
                FOC_EKP = OBSW_KP_GAIN_RUN3;                            // 估算器里的PI的KP
                FOC_EKI = OBSW_KI_GAIN_RUN3;                            // 估算器里的PI的KI  
             }
            break;
        default:
            break; 
    }
}

/*  -------------------------------------------------------------------------------------------------
    Function Name  : RampFucntion
    Description    : PLL启动时参数爬坡函数
    Date           : 2022-07-23
    Parameter      : None
    ------------------------------------------------------------------------------------------------- */

void RampFucntion(void)
{
    if (mcFocCtrl.CtrlMode == 0)
    {
        if (PLLfunction.BEMFBase_Update_Count > 2)
        {
            if  (PLLfunction.Switch_Count <= 400)
            {
                 mcFocCtrl.mcIqref = FOC_IQREF;
                if ( mcFocCtrl.mcIqref < IQ_Start_CURRENT)              //启动电流爬坡函数
                {
                    mcFocCtrl.mcIqref += 5;
                }
                
                FOC_IQREF = mcFocCtrl.mcIqref;
            }
            
            
        if (PLLfunction.BEMFBase_Update_Count1 < 2000)
        {
            PLLfunction.BEMFBase_Update_Count1++;
        }            
        if (PLLfunction.BEMFBase_Update_Count1 < 10)
        {
            if (PLLfunction.BEMFBase < V_factor1_Max)
            {
                PLLfunction.BEMFBase = PLLfunction.BEMFBase + 10;
            }
        }
        else
        {
            if (PLLfunction.BEMFBase < V_factor2_Max)
            {
                PLLfunction.BEMFBase = PLLfunction.BEMFBase + 10;
            }
        }            

            
            PLLfunction.BEMFBase_Update_Count = 0;
            MUL3_MB = PLLfunction.BEMFBase;
        }
        
        PLLfunction.BEMFBase_Update_Count++;
        PLLfunction.UBASE_Factor = (float)(mcFocCtrl.mcDcbusFlt) * 16383 / ((float)(U_BASE));
        if (PLLfunction.UBASE_Factor < 10000)
        {
            PLLfunction.UBASE_Factor = 10000;
        }
        MUL1_MA = PLLfunction.UBASE_Factor;
        SMDU_RunBlock(1,S1MUL);
        MUL2_MB = MUL1_MCH;
    }
}

void PI_PLL_Init(void)
{
    PI3_KP  = PLLKP_GAIN;
    PI3_KI  = PLLKI_GAIN;
    PI3_UKH = 0;
    PI3_UKL = 0;
    PI3_EK  = 0;
    PI3_EK1 = 0;
    PI3_UKMAX = 32767;
    PI3_UKMIN = -32767;

    PI2_KP  = PLLKP_GAIN;
    PI2_KI  = PLLKI_GAIN;
    PI2_UKH = 0;
    PI2_UKL = 0;
    PI2_EK  = 0;
    PI2_EK1 = 0;
    PI2_UKMAX = 32767;
    PI2_UKMIN = -32767;
}